%% Descent Rate Regulation Guidance Law for Gravity Turn
% 对应着陆的快速调整段，快速调整姿态到近似垂直
% CE-3高度从3km降到约2.4km，耗时约16s，角度转到45°
close all;
clear;
clc;

%% 参数初始化
% 张洪华等.嫦娥三号着陆器动力下降的制导导航与控制.中国科学:技术科学,2014.
% 部分初始数据采用TAPDG中kr=6的数据
% 天体参数
GM = 4.903E+12;     %月球引力常数(m^3/s^2)
RLunar = 1738E+3;   %月球平均半径
% 推力参数
Fmax = 7523;        %N
m0 = 1489;          %kg
Isp = 308;          %推力器比冲(s)
gE = 9.8;           %地表重力加速度(m/s^2)
C = Isp*gE;
% 初始条件，15km高度的椭圆轨道，轨道在惯性系xoz面内，假设已脱离轨道
h0 = 3E+3;
R0 = [450603.957523489; 0; 1681676.86356927];
V0 = [63.9022732277770; 0; -29.1862953215456];
A0 = [-2.21159589100805; 0; 2.93587226210146];
% 终端条件，轨迹在xoz面内从Z轴出发向X轴转
hf = 2.4E+3;
dhf = 0;
% 设定ODE45递推参数
dt = 0.1;
Dt = 0;
option = odeset('RelTol', 1e-7);
tspan = [0 dt];
% 画图所需数据
time = 0;
Position = R0;
Velocity = V0;
AThruster = zeros(3,1);
distance = 0;
altitude = h0;
velNorm = norm(V0);
aThrusterNorm = 0;
mass = m0;
FThruster = 0;
Gama = 0;

%% 制导过程
% 初始化
Xk = [R0; V0; m0];
aT = A0;
oldh = h0;
while 1
    % 动力学推导
    [tOrbit, xOrbit] = ode45(@(t,X) DescentDynamic(t, X, aT, GM, C), tspan, Xk, option);
    Xk = xOrbit(end,:)';
    R = Xk(1:3);
    V = Xk(4:6);
    m = Xk(7);
    aTmax = Fmax / m;

    % 制导律
    gvector = -R / norm(R);
    gnorm = GM / norm(R)^2;
    vvector = V / norm(V);
    gama = acos(dot(vvector,gvector)) - pi/2;
    h = norm(R) - RLunar;
    dh = (h-oldh) / dt;
    oldh = h;
    aT = vvector / sin(gama) * ((dhf^2-dh^2) / (2*(hf-h)) + gnorm);
    
    % 计算剩余时间
    Tgo = 2 * (hf - h) / (dhf + dh);
    Dt = Dt + dt;
    
    % 推力限制
    aTvector = aT / norm(aT);
    if norm(aT) > aTmax
        aT = aTmax * aTvector;
    end

    % 计算绘图所需量
    time = [time, Dt];
    Position = [Position, R];
    Velocity = [Velocity, V];
    AThruster = [AThruster, aT];
    distance = [distance, norm(R-R0)];
    altitude = [altitude, norm(R) - RLunar];
    velNorm = [velNorm, norm(V)];
    aThrusterNorm = [aThrusterNorm, norm(aT)];
    mass = [mass, m];
    FThruster = [FThruster, norm(aT)*m];
    Gama = [Gama, gama/pi*180];

    if Tgo < 1e-1
        break;
    end
    if norm(R) - RLunar < hf
        break;
    end
    if gama < -pi/2 + 1e-12
        break;
    end
end
% 用于调试速度收敛
% velNorm(end)

%% 画图
% csvwrite('drrg.csv', [time; distance; altitude; velNorm; aThrusterNorm; mass; FThruster]);
figure;
plot(time, altitude);
xlabel('时间(s)');
ylabel('高度(m)');
figure;
plot(time,Velocity);
xlabel('时间(s)');
ylabel('三轴速度(m/s)');
legend('vx', 'vy', 'vz');
figure;
plot(time(2:end), AThruster(:,2:end));
xlabel('时间(s)');
ylabel('三轴加速度(m/s^2)');
legend('ax', 'ay', 'az');
figure;
plot(time, velNorm);
xlabel('时间(s)');
ylabel('速度(m/s)');
figure;
plot(time(2:end), aThrusterNorm(2:end));
xlabel('时间(s)');
ylabel('加速度(m/s)');
figure;
plot(time, mass);
xlabel('时间(s)');
ylabel('着陆器质量(kg)');
figure;
plot(time(2:end), FThruster(2:end));
xlabel('时间(s)');
ylabel('推力(N)');
figure;
plot(distance, altitude);
xlabel('水平移动距离(m)');
ylabel('高度(m)');
figure;
plot(time(2:end), Gama(2:end));
xlabel('时间(s)');
ylabel('航迹角(°)');

%% 动力学模型
% 不考虑中心天体自转影响
% 不考虑大气
function Y = DescentDynamic(t, X, aT, GM, C)
    R = X(1:3);
    V = X(4:6);
    m = X(7);
    d_R = V;
    gvector = -R/norm(R);
    gnorm = GM / norm(R)^2;
    G = gvector * gnorm;
    d_V = aT + G;
    dm = -norm(aT)*m / C;
    Y = [d_R; d_V; dm];
end
